// Measurement covariance noise.
float rAngle = 0.03;

// Process covariance noise. 
float qAngle = 0.001;
float qGyro = 0.003;

void predictValues(float dt, float u, float * y,
FilterData * data) {
  data->x0 += dt * (u - data->x1);

  if (data->x0 >= 50) {
    while (abs(data->x0 - *y) > 300) {
      *y += 360;
    }
  } 
  else {
    while (abs(data->x0 - *y) > 300) {
      *y = *y - 360;
    }    
  }

  data->p00 += -dt * (data->p10 + data->p01) + qAngle * dt;
  data->p01 += -dt * data->p11;
  data->p10 += -dt * data->p11;
  data->p11 += qGyro * dt;
}

float updateValues(float y, FilterData * data) {
  const float innovation = y - data->x0;

  const float s = data->p00 + rAngle;

  const float k0 = data->p00 / s;
  const float k1 = data->p10 / s;

  data->x0 += k0 * innovation;
  data->x1 += k1 * innovation;

  data->p00 -= k0 * data->p00;
  data->p01 -= k0 * data->p01;
  data->p10 -= k1 * data->p00;
  data->p11 -= k1 * data->p01;

  return data->x0;
}

float getKalman(float dt, float u, float y, FilterData * data) {
  dt = dt / 1000;

  predictValues(dt, u, &y, data);
  return updateValues(y, data);
}

float getAccKalman(StateData * state, float measurement) {
  //prediction update
  //omit x = x
  state->p = state->p + state->q;

  //measurement update
  state->k = state->p / (state->p + state->r);
  state->x = state->x + state->k * (measurement - state->x);
  state->p = (1 - state->k) * state->p;  

  return state->x;
}

